
import com.cyberbotics.webots.controller.DifferentialWheels;
import com.cyberbotics.webots.controller.DistanceSensor;

public class BangBangFollowWall extends DifferentialWheels {
	private static final int THRESHOLD = 1100;

	private static int TIME_STEP = 16;

	private static int S_LEFT = 0; // Sensor left
	private static int S_MID_LEFT = 1; // Sensor middle left
	private static int S_FRONT_LEFT = 2; // Sensor front left
	private static int S_FRONT_RIGHT = 3; // Sensor front right
	private static int S_BACK_LEFT = 4; // Sensor front right

	//Arrays which hold the values for the drive()-method
	private static int[] SLOWLEFT = { 475, 525 };
	private static int[] HARDLEFT = { 400, 600 };
	private static int[] SLOWRIGHT = { 525, 475 };
	private static int[] MEDIUMRIGHT = { 575, 425 };
	private static int[] FASTFORWARD = { 1000, 1000 };

	private static int CARE_LOW = 450; // min. motor speed
	private static int CARE_HIGH = 550; // max. motor speed

	private DistanceSensor[] _distanceSensors; // Array with all distance sensors

	/**
	 * Constructor
	 */
	public BangBangFollowWall() {
		super();
		initSensors();
	}

	protected void initSensors() {
		// get distance sensors and save them in array
		_distanceSensors = new DistanceSensor[] { getDistanceSensor("ps5"), getDistanceSensor("ps6"), getDistanceSensor("ps7"), getDistanceSensor("ps0"), getDistanceSensor("ps4") };
		for (int i = 0; i < 5; i++) {
			_distanceSensors[i].enable(10);
		}
	}

	private void drive(int[] values) {
		setSpeed(values[0], values[1]);
	}

	private void turnLeft() {
		setSpeed((-1) * CARE_LOW, CARE_HIGH);
	}

	private void turnRight() {
		setSpeed(CARE_LOW, (-1) * CARE_HIGH);
	}

	/**
	 * User defined function for initializing and running the BangBangFollowTheWall class
	 */

	private void align() {
		int status = 0;
		while (step(TIME_STEP) != -1) {
			if (status == 0 && _distanceSensors[S_FRONT_LEFT].getValue() < THRESHOLD && _distanceSensors[S_FRONT_RIGHT].getValue() < THRESHOLD) {
				// initial action, drive forward
				drive(FASTFORWARD);
			} else if (status == 0 && (_distanceSensors[S_FRONT_LEFT].getValue() > THRESHOLD || _distanceSensors[S_FRONT_RIGHT].getValue() > THRESHOLD)) {
				// found a wall in front of the bot, turn right
				status = 1;
				turnRight();
			} else if (status == 1) {
				// turn right till the bot is somewhat parallel to the wall
				if (_distanceSensors[S_MID_LEFT].getValue() > _distanceSensors[S_BACK_LEFT].getValue() + 300) {
					turnRight();
				} else if ((_distanceSensors[S_MID_LEFT].getValue() <= _distanceSensors[S_BACK_LEFT].getValue() + 300) && (_distanceSensors[S_LEFT].getValue() - 200 > _distanceSensors[S_MID_LEFT].getValue() || _distanceSensors[S_LEFT].getValue() - 200 > _distanceSensors[S_BACK_LEFT].getValue())) {

					status = 2;
				}
			} else if (status == 2) {
				// drive along the wall, try to keep a distance between 1200 and 1250
				if (_distanceSensors[S_FRONT_LEFT].getValue() > THRESHOLD || _distanceSensors[S_FRONT_RIGHT].getValue() > THRESHOLD) {
					// found wall in front, switch into status 0
					status = 0;
				} else if (_distanceSensors[S_MID_LEFT].getValue() < 75 && _distanceSensors[S_LEFT].getValue() < 100) {
					// the wall we were following ended, switch to status 3
					status = 3;
				} else if (_distanceSensors[S_LEFT].getValue() > 1250) {
					if (_distanceSensors[S_LEFT].getValue() > 1750) {
						drive(MEDIUMRIGHT);
					} else {
						drive(SLOWRIGHT);
					}
				} else if (_distanceSensors[S_LEFT].getValue() < 1200) {
					drive(SLOWLEFT);
				}
			} else if (status == 3) {
				// if we found an end of a wall ( T-Piece ) try to follow the form
				if (_distanceSensors[S_FRONT_LEFT].getValue() > THRESHOLD || _distanceSensors[S_FRONT_RIGHT].getValue() > THRESHOLD) {
					// found wall in front, switch into status 0
					status = 0;
				} else if (_distanceSensors[S_BACK_LEFT].getValue() > _distanceSensors[S_MID_LEFT].getValue()) {
					turnLeft();
				} else if (_distanceSensors[S_LEFT].getValue() > _distanceSensors[S_BACK_LEFT].getValue() && _distanceSensors[S_LEFT].getValue() > _distanceSensors[S_MID_LEFT].getValue()) {
					drive(FASTFORWARD);
				} else if (_distanceSensors[S_LEFT].getValue() > 100 && _distanceSensors[S_BACK_LEFT].getValue() > 100 && _distanceSensors[S_MID_LEFT].getValue() > 100) {
					// we are again somewhat parallel to a wall, switch to status 2
					status = 2;
				} else if (_distanceSensors[S_LEFT].getValue() < 1000) {
					drive(HARDLEFT);
				}
			}
		}
	}

	/**
	 * Main method - in this method an instance of the controller is created and the method to launch the robot is called.
	 *
	 * @param args
	 */
	public static void main(String[] args) {
		BangBangFollowWall controller = new BangBangFollowWall();
		controller.align();
	}

}